#include "TypesCommands.h"
#include "Types.h"
#include "CButtonHandler.h"
#include "CTimerHandler.h"
#include "CCommand.h"

CButtonHandler button1(2);          // Switch button 1 is connected to pin 2
CTimerHandler  timerOnline(5000);   // ARDUINO_ONLINE command is sent every 5 seconds
const int ENGINE_WORK_TIME = 1000;  // Engine works for 1 second after each command

int enginePin[ENGINE_COUNT];
CTimerHandler engineTimers[] = {CTimerHandler(ENGINE_WORK_TIME), CTimerHandler(ENGINE_WORK_TIME), CTimerHandler(ENGINE_WORK_TIME), CTimerHandler(ENGINE_WORK_TIME)};

int commandCount = 0;
int valueCount = 0;

void setup()
{
   Serial.begin(9600);

   enginePin[ENGINE_1] = 11;
   enginePin[ENGINE_2] = 12;
   enginePin[ENGINE_3] = 13;
   enginePin[ENGINE_4] = 10;

   button1.init();
   timerOnline.start();
   
   pinMode(enginePin[ENGINE_1], OUTPUT);
   digitalWrite(enginePin[ENGINE_1], LOW);
   
   pinMode(enginePin[ENGINE_2], OUTPUT);
   digitalWrite(enginePin[ENGINE_2], LOW);
   
   pinMode(enginePin[ENGINE_3], OUTPUT);
   digitalWrite(enginePin[ENGINE_3], LOW);

   pinMode(enginePin[ENGINE_4], OUTPUT);
   digitalWrite(enginePin[ENGINE_4], LOW);
}

void loop()
{
  // Buttons handling
  onButton1Event(button1.processButton());
  
  // Timers handling
  if (TIMER_FIRED == timerOnline.processTimer())
  {
     onTimerOnlineEvent();
  }

  for (int i=0; i<ENGINE_COUNT; i++)
  {
    if (TIMER_FIRED == engineTimers[i].processTimer())
    {
      digitalWrite(enginePin[i], LOW);
      engineTimers[i].stop();
    }
  }
  
  CCommand incomingCommand = readCommand();
  
  if ((COMMAND_PC_ENGINE_FORWARD == incomingCommand.mCommandType) ||
      (COMMAND_PC_ENGINE_BACKWARD == incomingCommand.mCommandType) ||
      (COMMAND_PC_ENGINE_LEFT == incomingCommand.mCommandType) ||
      (COMMAND_PC_ENGINE_RIGHT == incomingCommand.mCommandType)
  )
  {
    eEngineName engineName;
    if (COMMAND_PC_ENGINE_1 == incomingCommand.mCommandType) engineName = ENGINE_1;
    if (COMMAND_PC_ENGINE_2 == incomingCommand.mCommandType) engineName = ENGINE_2;
    if (COMMAND_PC_ENGINE_3 == incomingCommand.mCommandType) engineName = ENGINE_3;
    if (COMMAND_PC_ENGINE_4 == incomingCommand.mCommandType) engineName = ENGINE_4;

    // If speed = 0, engine should be stopped
//    if (0 == incomingCommand.mValue8_1)
//    {
      // TODO: turn off engine
//    }
//    else
    {
      // Engine could have different directions
      if (ENGINE_DIRECTION_BACKWARD == incomingCommand.mValue8_2)
      {
         // TODO: turn on engine
         controlEngine(engineName, ENGINE_DIRECTION_BACKWARD, 255);
         
         // TODO: start timer that will turn off engine after N seconds (if next command comes, engine would be working non-stop)
         engineTimers[engineName].start();
      } else if (ENGINE_DIRECTION_FORWARD == incomingCommand.mValue8_2)
      {
         // TODO: turn on engine
         controlEngine(engineName, ENGINE_DIRECTION_FORWARD, 255);
         
         // TODO: start timer that will turn off engine after N seconds (if next command comes, engine would be working non-stop)
         engineTimers[engineName].start();
      }
    }
  }

}

CCommand readCommand()
{
  if (Serial.available() >= 5)  // magic number 5 - length of command
  {
    unsigned char cmd[5];
    for (int i=0; i<5; i++)
    {
      cmd[i] = Serial.read();
    }
    
    return CCommand(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4]);
  }
  else
  {
    return CCommand(COMMAND_INVALID, 0,0,0,0);
  }
}

void onButton1Event(uint8_t btnValue)
{
   if (BUTTON_PRESS == btnValue)
   {
      // pressed 
      CCommand cmd(COMMAND_ARDUINO_BTNSW_1, 0,0,0, BUTTON_PRESS);
      cmd.send();
      
   } else if (BUTTON_RELEASE == btnValue)
   {
      // released
      CCommand cmd(COMMAND_ARDUINO_BTNSW_1, 0,0,0, BUTTON_RELEASE);
      cmd.send();
   }
}

void onTimerOnlineEvent()
{
   CCommand cmd(COMMAND_ARDUINO_ONLINE, valueCount+3, valueCount+2, valueCount+1, valueCount);
   cmd.send();

   valueCount++;
}

void controlEngine(eEngineName engineName, eEngineDirection engineDirection, unsigned char speed)
{
   
   if ((ENGINE_DIRECTION_FORWARD == engineDirection) || (ENGINE_DIRECTION_BACKWARD == engineDirection))
   {
      digitalWrite(enginePin[engineName], HIGH);
   }
}
